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1. Introduction 


This report describes a method for determining the attitude of a spin axis from rotational 
measurements between known locations. This solution was conceived as a method to enhance a 
GPS receiver on a spinning body. The method was made more general by only dealing with 
locations and directions and is potentially useful for a wider class of applications. The 
generalized problem has been solved and simulations have been run to verify the implementation 
of the solution. 

The ability of a single sensor to determine both position and attitude is appealing from a tactical 
perspective. Software modification and the addition of appropriate antennas to a GPS receiver 
would realize a measurement that includes position and attitude. This package would provide all 
the information needed for input to control mechanisms for improving the precision of 
projectiles, indirectly increasing the lethality. The increase in lethality is a direct result of 
control algorithms reducing the delivery error. The accuracy of these algorithms is a function of 
the position and attitude accuracy. Obtaining an acceptable estimate of the spin angle has been a 
stumbling block in this area and is often referred to as the “finding down” problem. The 
described method goes beyond “finding down” to solve for the attitude. 

Another example of a possible use of this method would be to find the attitude of a spacecraft. 
Using stars as known sources of energy and rotational information, the spin axis of the spacecraft 
could be found. Similarly, this method could be applied to estimate the earth’s or any other 
planet’s spin axis. It is also possible to imagine systems using information from many spectral 
bands to obtain the needed roll information. Both pressure waves and electromagnetic energy 
could be used; systems could combine magnetic, visual, radar, IR, and acoustic information to 
determine attitude. The method described has received U.S. Patent 7,388,538 Bl. 


2. Coordinate Estimation 


This section discusses some of the issues with GPS and coordinate adjustment. Coordinate 
adjustment is used within GPS receivers to process pseudorange measurements. 

Coordinate adjustment is central to precise estimation of location. Surveying and geodesy both 
have contributed to this body of literature. Wolf and Ghilani' describe the way coordinate 
estimation is used in surveying. Strang and Borre^ discuss the way coordinate adjustment is used 


'Wolf, P. R.; Ghilani, C. D. Adjustment Computations: Statistics and Least Squares in Surveying and CIS; John Wiley and 
Sons: New York, NY, 1997. 
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Strang, G.; Borre, K. Linear Algebra, Geodesy, and GPS; Wellesly-Cambridge Press: Wellesly, MA, 1997. 
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in geodesy. Thompson^ discusses how angles, recast as inner product measurements, can be 
used within the umbrella of coordinate adjustment. Finding equations for the measurements in 
terms of the unknown location establishes the necessary function. The gradient of each of these 
functions is used as a row in a least-squares observation matrix. Using an iterative process, the 
location can be estimated. 


3. Development Background 


GPS receivers have advanced in complexity and features. Of particular interest are the receivers 
containing hundreds of thousands of correlators. These correlators are used to search the 
frequency-time delay space and find the GPS signal associated with a given satellite. After 
satellites have been acquired, many of these correlators are idle. 

A spinning round will create a Doppler effect on the received GPS signal for any radially 
oriented, non-circumferential antenna. Spinning rounds typically use ring antennas to bypass 
this situation. Consider the situation with a spinning round using both a ring antenna and a patch 
(or spin-axis-aligned antenna.) The GPS receiver will use the ring antenna to acquire and track 
the GPS signals; thus, the signal tracking information is available within the receiver. The patch 
antenna will have a frequency shift associated with its received signal for its rotation cycle, 
except when it is moving perpendicular to the satellite direction. By using the tracking 
information from the ring antenna and interrogating the patch antenna, the point of alignment can 
be found. The idle correlators can be used for this task. Using this information and timing 
information, roll angles between satellites can be found. With knowledge of position and 
direction to each satellite and three such measurements of roll angle, it is possible to find the 
orientation of the spin axis. Also note that a directional antenna could be used, removing the 
need for a Doppler shift. Signal parameters like signal-to-noise ratio could also be used to find 
roll angles between satellites or beacons. 

The previous discussion gives a possible implementation of the method. The requirements are to 
get roll angles between known directions. To visualize roll angles, consider a half plane attached 
to a spin axis. As this plane rotates, points in the containing space enter and leave this plane; the 
roll angle between two points is the amount of roll between one point leaving this rotating plane 
and the other point entering. Figure 1 is a graphic representation of this idea. The green 
rectangle represents a plane of observation for the sensor. An object in this plane is observed. 

As the observing plane spins to the position of the yellow rectangle, another object is observed. 
The roll angle between these observations is recorded as the measurement. Due to the geometry. 


^Thompson, A. Advances in Applied and Computational Mathematics] Nova Science Publishers: New York, NY, 2006; 
Chapter 18. 
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Figure 1. Roll angle. 

the constraints imposed by the roll angles are sufficient to define the orientation or attitude of the 
spin axis. The following extreme situations are discussed in an attempt to clarify the nature of 
the information being used. Consider two directions or vectors and the plane formed by them. If 
the spin-axis vector is orthogonal to this plane, then the roll angle between the two directions is 
the same as the angle between the two directions. If the spin axis is in the same plane between 
the two directions, then the roll angle will be 180°, shown in figure 1 as the green and cyan 
rectangles. However, if the spin axis is located in the same plane so that both directions are to 
one side, the roll angle between the two will be 0°, as they will both be in the same half plane. 
The roll angle between two directions depends on the orientation of the spin axis. Three roll 
measurements will define the attitude of the spin axis; thus, four known directions are required. 
For GPS systems, four satellites are always in view. 

It is a small extension to consider range systems with beacons at known locations being used as 
known directions. Lasers, infrared, and other portions of the electromagnetic spectrum can be 
used as energy sources; obviously, stars could make reliable energy sources. Sound waves and 
pressure waves of various types can be used as long as the direction to the energy source is 
known. The ability to monitor the precise attitude of a spinning shaft has uses in commercial 
enterprise as well as military and space applications. 
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4. Nonlinear Least Squares 


A brief review of least squares will be given first. Start with the over-determined equation 
FX = Y, where X is the vector of unknown coordinates, Y represents measurements, and F is a 
matrix model of a known linear relationship. Then project both sides onto the column space of 
the matrix F. Multiply each side by the transpose of F and then, assuming X is multiplied by a 
square matrix of full rank, multiply each side by the inverse. The result will be 

X = {F‘F)-'F‘Y. (1) 


This result, discussed in many textbooks, will be used as the core of an iterative procedure for 
nonlinear least squares. Both F and Y are known values, while X represents the unknown 
parameters. 

Suppose the functional relationship, /, between the measurements and the coordinates (X) is 
known. This function is typically nonlinear. To start, the value ofneeds to be guessed or 
approximated. Denote this value as X^. The gradient matrix for the measurements with respect 
to the coordinates will now be denoted byF. If Xq is reasonably close to A, then the following 
linearized approximation is valid; 

f(X) = f(X,) + F^^(X-X,). (2) 

The left side is the measurement vector, while the first term on the right is the evaluation of the 
functional relationship at the point X^. Moving this term to the left side allows the interpretation 
of the left side as the deviation of the measurements from the approximated coordinates. The 
gradient evaluated at the point is multiplied by the unknown deviation from the approximation of 
the location. The previous equation can be written in the form Y = FX as follows: 

f{X)-f{X,) = F,^{X-X,). (3) 

Linear least squares can be used to solve for the location deviation. After the solution is added to 
the current approximation (and becomes the new current approximation), the process can be 
repeated until the left side gets close to 0. If this is understood, what remains is to discuss 
realizations of / and F for different measurements in terms of coordinates. Generally, within 
the F matrix, different types of measurements can be used; however, only roll angles will be 
considered. Each measurement will define a row of the matrix. 
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5. Roll Angle Measurements 


First note that all the quantities are veetors on the unit sphere. All directions are normalized. 

The unknown quantity is the vector representing the spin axis. Consider the ideal case where the 
sensor only observes energy in a half plane attached to the spin axis. Recall that the 
Gram-Schmidt process is a way to set up an orthogonal basis for a set of vectors. For two 
vectors, we can use this to create two orthogonal vectors. Consider two known directions and a 
spin axis. The roll angle is the angle between the orthogonal components of the known 
directions to the spin angle. First, each of these vectors will be described, and then their inner 
product will give the angle between them. Let x represent the unknown coordinates of the spin 
axis; let be two known unit directions. The orthogonal or radial components of each 

direction are 


r^ = - Id 


H ’X X 


^2 2 ^^2 ^ X ^ X^ 


where the brackets indicate the inner product function. Assume these new vectors are 
normalized using normalization factors n\ and n 2 . The inner product between these will yield the 
cosine of the angle between them. 


cos(6') = 


di-{d^,x )x d 2 -{d 2 ,x )x \_jd^-p^x d 2 -p 2 


where the inner products are defined by p^ and p 2 . In the sequel, the additional subscript 
indicates the component of the represented quantity. This can be written out as 

= ((^11 -A^i)(^21 - P2X\)+{dn- P\^2id22 -P2^2)+{dx2 “ .PAs )(^23 -P2^2))- (7) 


Next, this expression will be expanded and then rearranged into a more concise group of symbols. 
Temporarily ignoring the normalization factors, multiply out each of the terms as follows: 

= dud 2 i-Pid 2 iX, - P 2 d,,X,+ p,P 2 xl +fl?i2fl?22 - A«^22^2 - Pld^l^l + PxPlA 
+^13^23 -Pld2iX2-p2di2X,+PiP2xl 

Now the regrouping of terms results in 

dY ^d' 2 j I d' x^d'22 ^ d'^d22 1 \ d' 2 I d'22^^2 ^ d'22^2 / 

(9) 

- />2 ( ^1 Al + d^2^2 + ^13-^3 ) + + -^2 + -^3 ) • 
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This result can be written in terms of inner products as follows: 


= {d„d^)-p,{d^,x )-p^{d„x ) + PiP2\x IP- (10) 

Recall all the original vectors, including the sensor spin-axis vector, were considered to be 
normalized. The preceding equation can be recast as 

= {di,d2)-PlP2-P2Pl+PlP2- ( 11 ) 

Reintroducing the normalization factor, 

_^d^,d^ —p^P2 _{d^,d^ — i^d^,x ){d2,x ) (12) 

np2 np2 


Now, the relationship between the cosine of the roll angle and the coordinates of the normalized 
spin axis has been established. Using this functional relationship, the gradient of the cosine of 
the roll angle with respect to the coordinates of the unknown spin axis can be determined. Next, 
an expression for the partial with respect to the ith component of the spin axis is needed as 
follows: 


(^d^,d2^ (^d^,x ) 

V ^1^2 j 


_ d^.(^d2,x ')~^d2^{d^,x ) 


(13) 




npi2 


This assumes the normalization factors make negligible contributions to the partial derivative. 
Assuming the normalization factors are constant simplifies each least-squares iteration, since the 
partials associated with the normalization factors are ignored. Since the exact partial is not being 
used, there will be a suboptimal convergence. The tradeoff between the number of steps saved 
per iteration and the additional number of iterations needed can be used to analyze this situation. 
The numerator of the roll angle expression contains the necessary information; thus, it seems the 
normalization factors do not contain information pertinent to the roll angle. In some situations, 
the extra complexity due to the partials of the normalization factors may be tolerated; however, 
as the simulations have shown, the normalization factors can be treated as constants in the 
partial. Although this simplification reduces implementation complexity, it results in a 
suboptimal convergence. At this point, it is possible to form a row in the previously described F 
matrix that corresponds to the cosine of the observed roll angle. To estimate the three 
coordinates of the spin axis, a minimum of three rows is required. By dealing directly with the 
cosine of the angle, the partial is simplified in comparison to the partial of the angle with respect 
to the coordinates. This perspective is no problem mathematically. 
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6. Simulation 


A Matlab toolbox was designed to test these ideas and verify that the method would produee 
aceurate results. After eompleting this validation, the set of GPS toolbox functions from Kai 
Borre was used to test the method under realistic conditions. In these simulations, the orientation 
of the spin axis was found. The method works; what remains is to work out details with specific 
applications. Matlab code for the simulation is included as the appendix. 

7. Precise Derivative 


In a previous discussion of using roll angles to determine attitude, it was stated that a reduced 
form of the derivative could be used in the linearization step of the iterative process. The 
simplification was justified through the use of a simulation. This simplification is investigated to 
give a more quantitative perspective to the former qualitative statement. The expression under 
investigation follows and represents a roll angle about a spin axis (x) between two known 
directions as follows: 

{, d \ ; ^2 ) ~ (^1 ’ (^2 ’ /I /|\ 


Here the d terms are known directions of unit magnitude, and x is an unknown direction of unit 
magnitude. The terms in the denominator are both normalization factors and are as follows: 

n. = -(<i..x)x||. (i; 


Adding an additional subscript to identify the component, this can be written as 


+ d.2X2 + (i-jXjjXj + (^d.2 - (d-^x^ + Y 

{da - (dnX, + d,2X2 + d,2X2)x2 f 


or more concisely. 


J 2 \2 

m -{d,^x^+d.2X2+d,,x,)Xj) . 
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The partial of equation 17 is deseribed by the following equation; 


1 


^[dy -{d.,x, + <2X2 + d^,x,)Xj ) -Id^jXj - ^+ 2 ^ 

V 1 k*j 


( 4 -(<1^1+<-2^2^ 


ik 

+d,,x,)dyx, 


5x, 


• (18) 


The numerator of the first expression has the following derivative: 

d{d„d,)-{d„x){d„x) ^ ^^ 

dx- 


(19) 


Now that the individual pieees have been defined, they will be assembled using the following 
equation. First, let p represent the numerator of the first expression. The expression of interest 
ean be written as 


( 20 ) 


The partial derivative of it ean be found using the product rule as follows: 

dpp^n2^ dp _i _i _i _2 dp 


dx, 


-1 -1 -1 -2 -1 -2 dn2 

-n^ 172 -P^l «1 T - P^l «2 ^ 

dX: dX: dx, 


( 21 ) 


This can now be evaluated by substituting the former expressions in the previous equation. If the 
directions and spin axis are known, then each of the terms will indicate the magnitude of the 
particular partial’s influence in relation to the total. If the second and third terms are relatively 
large, this can differ significantly from the abbreviated partial used in the simulation. The 
appendix includes code for evaluating the partial. 

A simulation was designed to compare the partial of p with the partial of . The measure of 

77j772 

agreement was the inner product of the normalized partials. If they both point in the same 
direction, the inner product is I; if the inner product is 0, the partials are perpendicular, and 
negative values will result in divergence in the estimation process. Three cases were 
investigated. In the first case, the two known directions and the spin axis were chosen randomly. 
The results are shown in figure 2. 

Although the majority of the cases are in the correct direction and thus lead to convergence, there 
are instances approaching complete divergence. From figure 2, it can be conjectured that using 
the partial of p will result in suboptimal convergence that will take a step in the wrong direction 
on -10% of the iterations and move in the appropriate direction on -70% of the steps, as 
indicated by positive inner products. 
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Figure 2. Agreement for randomly chosen directions and spin axis. 

A second simulation was performed with the two known directions perpendicular to each other. 
Figure 3 illustrates the results of the simulation. Comparing this to figure 2, it can be seen that 
the number and magnitude of divergent instances have decreased. In this case, ignoring the 
partials of the normalization factors has a smaller effect on the convergence of the estimator, and 
less than 2% of the cases are divergent. Figure 4 shows that if the two known directions are 120° 
apart, 10% of the cases diverge. 


Perpendicular Directions 

500 r i ^ i ^ 



Inner Product 


Figure 3. timer products for perpendicular directions. 
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Figure 4. Inner produets for 120° separation of directions. 

Although the estimator converged while ignoring the partials of the normalization factors, the 
results of this section indicate it may be at the cost of extra iterations. An implementation of the 
method that does not include the partials of the normalization factors should present a rationale 
for their omission. 


8. Closing Remarks 


This method can have widespread application. Attitude estimation is a problem in many fields, 
from detecting small shifts in snow to predicting avalanches to aerospace applications. Many 
systems’ sole purpose is to estimate the attitude of an object. A method that estimates attitude 
from known directions and roll angle measurements has been developed, and simulations show 
that if the requirements are met, accurate estimates of the axis orientation can be made. For 
artillery systems, this is a solution to the “find down” problem. For systems already equipped 
with GPS for position information, this will allow the receivers to be extended and also deliver 
attitude information. This method can be used on a spinning sensor where the directions to 
known sources of energy are known. It would also be possible to simulate a spin axis with a 
steerable antenna. A single sensor providing both location and attitude information would solve 
many tactical and test range problems. 
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Appendix. Matlab Code 
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function [dip, num]=ipiter(a,meas) 

%for i=l;n a(:,i)=a(:,i)/norm(a(:,i)); end 
aip=a'*a; %inner products 
[m n]=size(a); 

%sweep the spin axis eomponent from the other veetors 
for i=2:n 

sa(: ,i)=a(: ,i)-aip( 1 ,i) * a(:, 1); 
end 

anonn=sqrt(sum(sa.^2)); %norm of the veetors orth to spin axis 
%get the measurements e notes 
num=anorm; 
for i=2:n-l 

num(i)=(aip(i,i+1 )-aip( 1 ,i) * aip( 1 ,i+1))/(anorm(i)* anorm(i+1)); 
end 

%e the angular situation 
%acos(num)* 180/pi; 

% find differential 
dip=zeros(n-2,3); %initialize 
for j=2:n-l 

denom=anorm(j ) * anorm(j+1); 
for i=l;3 

dipO-1 ,i)=-(a(i,j)*aip( 1 ,j+l)+a(i,j+ l)*aip( 1 ,j))/denom; 
end 
end 

routine ipeonverge 

tst=astart(:,l)'; 

%a(:,l)=[-.2;-.9;.0001]; 

it=0; 

[r n_ipe]=size(meas); 
n_ipe=n_ipe-l; 

while (1 -abs(tst*a(:,!))>.000001) 

%for i=l;20 
it=it+l; 

[dip num]=ipiter(a,meas); 
y=(meas(2:n_ipe)-num(2:n_ipe))'; 
xtx=inv(dip'*dip); 
yadj=xtx*dip'*y; 
a(:,l)=a(:,l)+yadj; 
a(:,l)=a(:,l)/norm(a(:,l)); 
if it>100 break; end 
end 
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%function y=gpsipsim() 


close all; 
clear all; 


% - 

% — Read ephemeris (RINEX nav-file or YUMA almanac) 
% — and delete double occurences of SVN's 
%- 


eph = sortrows(rdyuma('yuma79.txt')); 


% - 

% — Station coordinates (WGS84 / XYZ) — 
%- 


station(l) = 3923625; % Delft (Geodesy-building) 
station(2) = 298462; 
station(3) = 5002803; 

cutoff =10; % Cutoff elevation in degrees 


o/o- 

% — Time/span — 
%- 


tsat = mktsat('28-FEB-2001 12;00;00','28-FEB-2001 14;00;00',300); 


% - 

% — Compute satellite positions (XYZ WGS84 & AZ/EL) — 
%- 


[xsat,ysat,zsat,azim,elev,rotmat] = cpaziele (tsat,eph,station); 

%get local directions form the rotation matrix 
loc al_vert=-rotmat(3,:)'; 
local_north=rotmat( 1,:)'; 

%pick a random angle to rotate about the local vert 

%make a quaternion rotation about the vert 

%find the new axis by rotating local north about the vert 

d_r=2*pi*rand; 

q_d=q_aa2quat(local_vert,d_r); 

axis=q_rotate(q_d,local_north); 

%follow above procedure choose rotation 

%make rotation quat to rotate about previous horizantal axis 

%calculate new axis by rotaing the verticle axis 

d_r=45*pi/180; 
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q_d=q_aa2quat(axis,d_r); 

s_axis=q_rotate(q_d,local_vert); 

a=zeros(3,29); 

[r n_epochs]=size(tsat); 

con_its=[]; 

est_err=[]; 

dxtx=[]; 

itlist=[]; 

i_noncon=0; 

for i=l:n_epochs 

vis_sat = find (elev(:,i) > cutoff); 

n_direct=size(vis_sat); 

for j=l :size(vis_sat) 

tmp=[xsat(vis_sat(j),i);ysat(vis_sat(j),i);zsat(vis_sat(j),i)]; 
tmp=tmp-station'; 
tmp=tmp/norm(tmp); 
a(:,j)=tmp; 
end 

a=[s_axis a]; 
n=n_direct+l; 
a=a(:,l:n); 
aip=a'*a; 

for i=2:n 

sa(:,i)=a(:,i)-aip(l,i)*a(:,l); 

end 

anorm=sqrt(sum(sa.^2)); 

num=a(l,:); 

aip=a'*a; 

for i=2:n_direot 

num(i)=(aip(i,i+1 )-aip( 1 ,i)* aip( 1 ,i+1 ))/(anorm(i)* anorm(i+1)); 
end 

meas=num; 

astart=a; 

d_r=2*pi*rand; 

q_d=q_aa2quat(s_axis,d_r); 

axis3=q_rotate(q_d,axis); 

d_r=10/180*pi; 

q_d=q_aa2quat(axis3 ,d_r); 

axis_p=q_rotate(q_d,s_axis); 

a(:,l)=axis_p; 

ipeonverge 

eon_its=[eon_its it]; 

err=astart(:, 1 )-a(:, 1); 

est_err=[est_err err]; 

dxtx=[dxtx det(xtx)]; 
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ifit>100 

i_noncon=i_noncon+1; 
noncon(i_noncon). a=astart; 
noncon(i_noncon). sa=axis_p; 
noncon(i_noncon).meas=meas 
i 

xtx 

anorm 

aip 

a 

end 

end 

example of use 

gpsipsim 

ipinvest 

a=noneon.a 

ipinvest 

ploths 

a=noneon.a 

r_set 

ploths 

eonits 

gpsipsim 

eonits 

help find 

fmd(eon_its> 100) 

noneon 

size(nonoon) 

nonconinv 

ploths 

pploths 

ploths 

noneon 

r_nonoon=nonoon 

eonits 

rotatenoeon 

rotatnoeon 

rotatnoneon 

nonoontemp=nonoon 

none on=r_nono on; 

nonconinv 

ploths 



%function y=ipadjust() 


n=5; %n must b more than 3 
a=randn(3,n); 

for i=l:n a(:,i)=a(:,i)/norm(a(:,i)); end 
aip=a'*a %mner products 

%sweep the spin axis component from the other vectors 
for i=2;n 

sa(: ,i)=a(: ,i)-aip( 1 ,i) * a(:, 1); 
end 

anorm=sqrt(sum(sa.^2)); %norm of the vectors orth to spin axis 
%get the measurements c notes 
num=anorm; 
for i=2;n-l 

num(i)=(aip(i,i+1 )-aip( 1 ,i) * aip( 1 ,i+1))/(anorm(i)* anorm(i+1)); 
end 

%c the angular situation 
acos(num)* 180/pi 

% find differential 
dip=zeros(n-2,3); %initialize 
for j=2;n-l 

denom=anorm(j ) * anorm(j+1); 
for i=l;3 

dipO-1 ,i)=-(a(i,j)*aip( 1 ,j+1 )+a(i,j+1 )*aip( 1 ,j))/denom; 
end 
end 

%functiony=ipconverge_d(d,rotaxis) 


d_r=d/180*pi 

q_d=q_aa2quat(rotaxis,d_r) 

axis=q_rotate(q_d,v) 

a(:,l)=axis 

ipconverge 

y=it 


%function y=ipinvest(a) 
aip=a'*a %inner products 
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%sweep the spin axis component from the other vectors 

[r n]=size(a); 
for i=2;n 

sa(: ,i)=a(: ,i)-aip( 1 ,i) * a(:, 1); 
end 

anorm=sqrt(sum(sa.^2)); %norm of the vectors orth to spin axis 
%get the measurements c notes 
num=anorm; 
for i=2;n-l 

num(i)=(aip(i,i+1 )-aip( 1 ,i) * aip( 1 ,i+1))/(anorm(i)* anorm(i+1)); 
end 

astart=a; 

meas=num; 

tst=astart(:,l)'; 

a(:,l)=starta 

% 

it=0; 

[r n_ipc]=size(meas); 

n_ipc=n_ipc-l; 

seq=[]; 

while (1 -abs(tst*a(:,!))>.000001) 

%for i=l;20 
it=it+l; 

[dip num]=ipiter(a,meas); 
y=(meas(2:n_ipc)-num(2:n_ipc))'; 
xtx=inv(dip'*dip); 
yadj=xtx*dip'*y; 

%af=yadj-yadj'*a(:,l)*a(:,l); this method oscilated toom uch 
%y adj_ 1 =norm(y adj ) * af/norm(af); 
temp=a(:,l)+yadj; %method2 
temp=temp/norm(temp); %m2 
t_2=temp-a(:,l); %m2 
t_2=norm(yadj)/norm(t_2)*t_2; %m2 
a(:,l)=a(:,l)+in_2; %m2 
a(:,l)=a(:,l)/norm(a(:,l)); %m2 
%a(:,l)=temp/norm(temp); %method 1 
seq=[seq a(;,l)]; 
if it>100 break; end 
end 

figure;plot3(seq(l,:),seq(2,:),seq(3, 
grid on; 

hold on;plot3(astart(l,l),astart(2,l),astart(3,l),'ro') 
plot3(seq( 1,1 ),seq(2,1 ),seq(3,1 ),'go') 
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%this was to plot eigenvalues from [d v]=eig(xtx) 

%x=astart(:,l);for iz=l:3 plot3([x(l) x(l)+.02*v(l,iz)],[x(2) x(2)+.02*v(2,iz)], [x(3) 
x(3)+.02*v(3,iz)]); end 

%funetion y=ipstart() 

ipadjust 

astart=a; 

meas=num 

v=astart(:,l) 

vperp=[v(2);-v(l);0] 

deg=2*pi*rand 

pertaxis=q_aa2quat(v,deg) 

rotaxis=c[_rotate(pertaxis,vperp) 

d5=5/180*pi 

q5=q_aa2quat(rotaxis, d5 ) 
axis_5=q_rotate(q5 ,v) 
a(:,l)=axis_5 
ipeonverge 

dl0=10/180*pi 
ql 0=q_aa2quat(rotaxis,dl 0) 
axis_l 0=q_rotate(ql 0,v) 
a(:,l)=axis_10 
ipeonverge 

%funetion y=iptest() 

n=3; %n must b more than 3 
a=randn(3,n); 

for 1=1 ;n a(:,i)=a(:,i)/norm(a(:,i)); end 
a; 

a2=a.^2; 

sum(a2) 

aip=a'*a 

ipl=aip(l,3); 

ip2=aip(2,3); 

sal=a(:,l)-ipl*a(:,3); 

sar*a(:,3) 

sa2=a(:,2)-ip2*a(:,3); 

s_ip=sa 1' * sa2/(norm(sa 1) *norm(sa2)); 
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acos(s_ip)* 180/pi 
acos(aip( 1,2))* 180/pi 

sa2'*a(:,3) 

nsl=norm(sal); 

ns2=norm(sa2); 

num=aip(l,2)-aip(l,3)*aip(2,3); 

acos(num/(nsl *ns2))* 180/pi 

% find differential 

dip=sal; 

for i=l;3 

dip(i)=-(a(i,l)*aip(2,3)+a(i,2)*aip(l,3)); 

end 

dip=dip/(nsl *ns2) 


%function y=nonconinv() 
[r e]=size(noncon); 


for i=l:c 

a=nonoon(i).a 

starta=nonoon(i).sa 

ipinvest 

end 

%funetion y=plot_hs() 
n=20 

theta = pi*(-n;2;n)/n; 
phi = (pi/2)*(0:2;n)'/n; 

X = cos(phi)*cos(theta); 

Y = cos(phi)*sin( theta); 

Z = sin(phi)*ones(size(theta)); 
%figure 

plot3(X,Y,Z,y) 

[r,c]=size(Z); 
hold on 
for i=l:r 

plot3(X(i,:),Y(i,:),Z(i,:),y) 

end 

%funetion y=r_set() 
q 1 =q_aa2quat( [ 1; 1 ;0] ,pi/4) 
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ar=a;for j=l:9 ar(:,j)=q_rotate(ql,a(:,j)); end 

a=ar 

ipinvest 

%function y=rotatnoncon() 
r_noncon=noncon; 

ql=q_aa2quat([l;l;0],pi/4); %rotational axis and angular rotation 
[r c]=size(noncon); 


for i=l;c 

ar=noncon(i).a;for j=l:9 ar(:,j)=q_rotate(ql,a(:,j)); end 
r_noneon(i).a=ar; 

r_noneon(i) .sa=q_rotate(q 1 ,noneon(i). sa); 
end 
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